/*
 * @Author: _oufen
 * @Date: 2023-07-03 13:31:06
 * @LastEditTime: 2023-07-19 08:56:01
 * @Description:   pid相关函数
 */
#include "pid.h"
#include "encoder.h"
#include "mycontrol.h"
#include "Tracking.h"
#include "ti/driverlib/dl_uart_main.h"
#include "ti_msp_dl_config.h"
#include "motor.h"

_pid pid_speed1, pid_speed2; // 速度环结构体变量

_pid pid_location; // 巡线环 结构体变量

float Pid_speed1, Pid_speed2, Pid_location; // 速度环的实际值

uint8_t U1_DataBuff[200];
uint16_t U1_RxLine = 0;
uint8_t U1_RxBuffer[1];
uint8_t data = 0;
float error = 0;
int m =0;
// void PID_param_init(void)
// {
//     /*左轮pid参数*/
//     pid_speed1.target_val = 0.0; // 期望速度 rpm
//     pid_speed1.actual_val = 0.0;
//     pid_speed1.err = 0.0;
//     pid_speed1.err_last = 0.0;
//     pid_speed1.integral = 0.0;

//     pid_speed1.Kp = 600.0;
//     pid_speed1.Ki = 10.0;
//     pid_speed1.Kd = 0.0;

//     /*右轮pid参数*/
//     pid_speed2.target_val = 0.0; // 期望速度  rpm
//     pid_speed2.actual_val = 0.0;
//     pid_speed2.err = 0.0;
//     pid_speed2.err_last = 0.0;
//     pid_speed2.integral = 0.0;

//     pid_speed2.Kp = 0.0;
//     pid_speed2.Ki = 0.0;
//     pid_speed2.Kd = 0.0;
// }

// float speed1_pid_control(float actual)
// {
//     pid_speed1.err = pid_speed1.target_val - actual;

//     // 消除机械误差
//     if ((pid_speed1.err < 0.5f) && (pid_speed1.err > -0.5f)) // 假如以最大允许速度偏差运行1分钟，输出轴最大偏差为半圈
//     {
//         pid_speed1.err = 0.0f;
//     }

//     // 误差累积
//     pid_speed1.integral += pid_speed1.err;

//     /*积分限幅 pid_speed1.Ki *限幅值等于PWM输出最大值*/
//     // 调试的时候需要注释  待确i后进行限幅
//     // if (pid_speed1.integral>= 4000)
//     // {
//     //     pid_speed1.integral= 4000;
//     // }
//     // else if (pid_speed1.integral< -4000)
//     // {
//     //    pid_speed1.integral= -4000;
//     // }

//     /*PID算法实现*/
//     pid_speed1.actual_val = pid_speed1.Kp * pid_speed1.err + pid_speed1.Ki * pid_speed1.integral + pid_speed1.Kd * (pid_speed1.err - pid_speed1.err_last);

//     /*误差传递*/
//     pid_speed1.err_last = pid_speed1.err;

//     return pid_speed1.actual_val;
// }

/*********************************************************************************************************************/

/**
 * @brief 	pid
 * @param 	无
 * @retval  无
 */

void PID_param_init(void)
{
    /*左轮pid参数*/
    pid_speed1.target_val = 0.0; // 期望速度 rpm
    pid_speed1.actual_val = 0.0;
    pid_speed1.err = 0.0;
    pid_speed1.err_last = 0.0;
    pid_speed1.integral = 0.0;

    pid_speed1.Kp = 68.0;
    pid_speed1.Ki = 0.0;
    pid_speed1.Kd = 7.0;

    /*右轮pid参数*/
    pid_speed2.target_val = 0.0; // 期望速度  rpm
    pid_speed2.actual_val = 0.0;
    pid_speed2.err = 0.0;
    pid_speed2.err_last = 0.0;
    pid_speed2.integral = 0.0;

    pid_speed2.Kp = 50.0;
    pid_speed2.Ki = 0.0;
    pid_speed2.Kd = 0.0;

    /*巡线环pid参数*/              // 舵机转向
    pid_location.target_val = 0.0; // 巡线中心值 自己进行设置  期望巡线中心值
    pid_location.actual_val = 0.0;
    pid_location.err = 0.0;
    pid_location.err_last = 0.0;
    pid_location.integral = 0.0;

    pid_location.Kp = 80.0;
    pid_location.Ki = 0.0;
    pid_location.Kd = 10.0;
}

/**
 * @brief   获取目标值
 * @param   无
 *	@note 	无
 * @retval  目标值
 */
float get_pid_target(_pid *pid)
{
    return pid->target_val; // 获得当前的目标值
}

/**
 * @brief   获取实际值
 * @param   无
 *	@note 	无
 * @retval  实际值
 */
float get_pid_actual(_pid *pid)
{
    return pid->actual_val;
}

/**
 * @brief   设置 P I D
 * @param   无
 *	@note 	无
 * @retval  无
 */
void set_p_i_d(_pid *pid, float p, float i, float d)
{
    pid->Kp = p; // 设置比例系数 P
    pid->Ki = i; // 设置积分系数 I
    pid->Kd = d; // 设置微分系数 D
}

/**
 * @brief   设置目标值
 * @param  val	目标值
 *	@note 	无
 * @retval 无
 */
void set_pid_target(_pid *pid, float temp_val)
{
    pid->target_val = temp_val; // 设置当前的目标值
}

/**
 * @brief  速度PID算法实现
 * @param  actual_val:实际值
 *	@note 	无
 * @retval 通过PID计算后的输出
 */
float speed_pid_realize(_pid *pid, float actual_val)
{
    /*计算目标值与实际值的误差*/
    pid->err = pid->target_val - actual_val;

    // 消除机械误差
    if ((pid->err < 0.5f) && (pid->err > -0.5f)) // 假如以最大允许速度偏差运行1分钟，输出轴最大偏差为半圈
    {
        pid->err = 0.0f;
    }

    pid->integral += pid->err; // 误差累积

    /*积分限幅 pid_speed1.Ki *限幅值等于PWM输出最大值*/ // 调试的时候需要注释  待确i后进行限幅
    // if (pid->integral >= 4000)
    // {
    //     pid->integral = 4000;
    // }
    // else if (pid->integral < -4000)
    // {
    //     pid->integral = -4000;
    // }

    /*PID算法实现*/
    pid->actual_val = pid->Kp * pid->err + pid->Ki * pid->integral + pid->Kd * (pid->err - pid->err_last);

    /*误差传递*/
    pid->err_last = pid->err;

    /*返回当前实际值*/
    return pid->actual_val;
}

/**
 * @brief  循迹PID算法实现   通过接收Mv发送过来的巡线
 * @param  actual_val:实际值
 *	@note 	无
 * @retval 通过PID计算后的输出
 */
// TIM0->CCR1 = 1400; // 初始化 舵机PWM大概在 700左右   初始化舵机  初始化角度在90 1.5ms 对应占空比为1400

// TIM0->CCR1 = 700;  舵机角度 左转最大角度
// TIM0->CCR1 = 2100; 舵机角度 右转最大角度
float location_pid_realize(_pid *pid, float actual_val)
{
    /*    计算偏差    这里的偏差是指 巡线偏差 设定的巡线期望值 和 MV传回的巡线实际值 得偏差 */
    pid->err = pid->target_val - actual_val;

    pid->integral += pid->err; // 误差累积

    // if (pid->err >= 1000) // 积分限幅
    // {
    //     pid->err = 0;
    // }

    // if (huidu.output >= 550)
    //     huidu.output = 550;
    // if (huidu.output <= -600)
    //     huidu.output = -600;

    /*PID算法实现*/
    pid->actual_val = pid->Kp * pid->err + pid->Ki * pid->integral + pid->Kd * (pid->err - pid->err_last);

    /*误差传递*/
    pid->err_last = pid->err;

    /*返回当前实际值*/
    return pid->actual_val;
}

/**
 * @brief  速度环pid输出函数
 * @param  actual_val:实际值
 *	@note 	无
 * @retval 通过PID计算后的输出
 */
float speed1_pid_control(void)
{

    float Expect_Pwm = 0.0; // 当前控制值

    // pid_speed1.actual_val = ((float)Param.UnitTime_Motor1Pluse * 1000.0 * 60.0) / (RESOULTION_TOTAL_RATIO * REDUCTION_RATIO * PID_PERIOD);

    // 四倍频
    //  Pid_speed1 = ((float)Param.UnitTime_Motor1Pluse * 60000.0) / 17680;     // MG-370
    //    Pid_speed1 = ((float)Param.UnitTime_Motor1Pluse * 60000.0) / 10400; // MG-310

    // 二倍频
    //Pid_speed1 = ((float)Param.UnitTime_Motor1Pluse * 60000.0f) / 2600.0f; // MG-310
    Pid_speed1 = (float)Param.UnitTime_Motor1Pluse * 0.038667f * 100.0f; // MG-310 m/s

    // 速度滤波后  效果不好
    // Pid_speed1 = Speed_Low_Filter(((float)Param.UnitTime_Motor1Pluse * 60000.0) / 10400, speed_Record1); // MG-310

    Expect_Pwm = speed_pid_realize(&pid_speed1, Pid_speed1); // 进行 PID 计算

    //  #if defined(PID_ASSISTANT_EN)
    //    set_computer_value(SEND_FACT_CMD, CURVES_CH1, &actual_speed, 1);                // 给通道 1 发送实际值
    //  #endif

    return Expect_Pwm;
}

/**
 * @brief  速度环pid输出函数
 * @param  actual_val:实际值
 *	@note 	无
 * @retval 通过PID计算后的输出
 */
float speed2_pid_control(void)
{

    float Expect_Pwm = 0.0; // 当前控制值

    // pid_speed1.actual_val = ((float)Param.UnitTime_Motor1Pluse * 1000.0 * 60.0) / (RESOULTION_TOTAL_RATIO * REDUCTION_RATIO * PID_PERIOD);

    // 四倍频
    // Pid_speed2 = ((float)Param.UnitTime_Motor2Pluse * 60000.0) / 17680;  //MG-370
    // Pid_speed2 = ((float)Param.UnitTime_Motor2Pluse * 60000.0) / 10400; // MG-310

    // 二倍频
    //Pid_speed2 = ((float)Param.UnitTime_Motor2Pluse * 60000.0f) / 2600.0f; // MG-310
    Pid_speed2 = (float)Param.UnitTime_Motor2Pluse * 0.038667f * 100.0f; // MG-310 m/s

    // 速度滤波后   效果不好
    // Pid_speed2 = Speed_Low_Filter(((float)Param.UnitTime_Motor2Pluse * 60000.0) / 10400, speed_Record2); // MG-310

    Expect_Pwm = speed_pid_realize(&pid_speed2, Pid_speed2); // 进行 PID 计算

    //  #if defined(PID_ASSISTANT_EN)
    //    set_computer_value(SEND_FACT_CMD, CURVES_CH1, &actual_speed, 1);                // 给通道 1 发送实际值
    //  #endif

    return Expect_Pwm;
}

/**
 * @brief  巡线pid输出函数      舵机的pwm
 * @param  actual_val:实际值
 *	@note 	无
 * @retval 通过PID计算后的输出
 */
float location_pid_control(void)//舵机PWM输出最大为5000
{

    float Expect_Pwm = 0.0; // 当前控制值

    //Pid_location = Tracking_Err_Get()*10; // 获取巡线模块当前误差

    //Pid_location = 0;

    Pid_location = error;//视觉循迹pid
    Expect_Pwm = location_pid_realize(&pid_location, Pid_location); // 进行 PID 计算
    //  #if defined(PID_ASSISTANT_EN)
    //    set_computer_value(SEND_FACT_CMD, CURVES_CH1, &actual_speed, 1);                // 给通道 1 发送实际值
    //  #endif

    return Expect_Pwm;
}

float Get_K210_Data(void)//接收K210的误差数据
{
    uint8_t data_Start_Num = 1; // 记录数据位开始的地方
    uint8_t data_End_Num = 0; // 记录数据位结束的地方
    uint8_t data_Num = 0; // 记录数据位数
    uint8_t minus_Flag = 0; // 判断是不是负数
    float data_return = 0; // 解析得到的数据

    for(uint8_t i=0;i<8;i++) // 查找等号和感叹号的位置,得到数字位数
    {
        //if(DataBuff[i] == '+' || DataBuff[i] == '-') data_Start_Num = i + 1; // +1是直接定位到数据起始位
        if(U1_DataBuff[i] == '\n')
        {
            data_End_Num = i - 1;
            break;
        }
    }
    if(U1_DataBuff[0] == '-')
    {
        minus_Flag = 1;
    }
    // if(DataBuff[data_Start_Num] == '-') // 如果是负数
    // {
    //     data_Start_Num += 1; // 后移一位到数据位
    //     minus_Flag = 1; // 负数flag
    // }
    data_Num = data_End_Num - data_Start_Num + 1;
    if(data_Num == 4) // 一位整数加小数
    {
        data_return = (U1_DataBuff[data_Start_Num]-48)  + (U1_DataBuff[data_Start_Num+2]-48)*0.1f +
                (U1_DataBuff[data_Start_Num+3]-48)*0.01f;
    }
    else if(data_Num == 5) // 两位整数加小数
    {
        data_return = (U1_DataBuff[data_Start_Num]-48)*10 + (U1_DataBuff[data_Start_Num+1]-48) + (U1_DataBuff[data_Start_Num+3]-48)*0.1f +
                (U1_DataBuff[data_Start_Num+4]-48)*0.01f;
    }
    if(minus_Flag == 1)  data_return = -data_return;
    return data_return;
}

void USART_K210(void)//解析K210接收来的数据
{
    float data_Get = Get_K210_Data(); // 存放接收到的数据
    error = data_Get;
}

void UART_3_INST_IRQHandler(void) //opencv识别数字
{
    switch (DL_UART_Main_getPendingInterrupt(UART_3_INST))
    {
        case DL_UART_MAIN_IIDX_RX:
        {
            U1_RxLine++;                      //每接收到一个数据，进入回调数据长度加1
            U1_RxBuffer[0] = DL_UART_Main_receiveData(UART_3_INST);//接收目标数据
            U1_DataBuff[U1_RxLine-1]=U1_RxBuffer[0];  //把每次接收到的数据保存到缓存数组

            if(U1_RxBuffer[0]=='\n')            //接收结束标志位
            {
                 for(int i=0;i<U1_RxLine;i++)
                 {
                    // printf("UART DataBuff[%d] = %c\r\n",i,U1_DataBuff[i]);
                 }
                USART_K210();
                Pid_location = error;
                memset(U1_DataBuff,0,sizeof(U1_DataBuff));  //清空缓存数组
                U1_RxLine=0;  //清空接收长度
                U1_RxBuffer[0]=0;//清空接收缓冲
            }
        }
            break;
        default:
            break;
    }
}
